/*
 *  Copyright (c) 2018, Infineon Technologies AG
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification,are permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *  this list of conditions and the following disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *  this list of conditions and the following disclaimer in the documentation
 *  and/or other materials provided with the distribution.
 *  - Neither the name of the copyright holders nor the names of its contributors
 *  may be used to endorse or promote products derived from this software without
 *  specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  To improve the quality of the software, users are encouraged to share
 *  modifications, enhancements or bug fixes with Infineon Technologies AG
 *  dave@infineon.com).
 */
/**
 * \defgroup controller Controller
 * @{
 */
/**
 * \file    controller_boost.h
 * \author  Manuel Escudero Rodriguez
 * \date    09.05.2018
 * \brief   Controller code
 */
#ifndef __CONTROLLER_BOOST_H_
#define __CONTROLLER_BOOST_H_

#include "controller.h"
#include "config_adc.h"
#include "config_boost_pwm.h"

/**
 * \name Fixed point compensator boost mode
 * Compensator bandwidth .
 * @{
 */
/** Output voltage reference value. [ADC_units] */
#define BOOST_VOLTAGE_REF 		(10020)//(9500)-->360V (10020)//-->380V (10580) // -->400V
#if ((BOOST_VOLTAGE_REF < BOOST_VIN_MAX_LOW) || (BOOST_VOLTAGE_REF < BOOST_VIN_MIN_HIGH))
	#error "BOOST_VOLTAGE_REF should be over starting up output voltage range"
#endif
/** Fixed point format of coefficients. [bits] */
#define BOOST_Q_DECIMALS  		(14)
/** Q0.Q_DECIMALS format coefficient. */
#define BOOST_COEFF_B2    		((0 << BOOST_Q_DECIMALS) + 2867)
/** Q0.Q_DECIMALS format coefficient. */
#define BOOST_COEFF_B1    		(-((0 << BOOST_Q_DECIMALS) + 4295))
/** Q0.Q_DECIMALS format coefficient. */
#define BOOST_COEFF_B0    		((0 << BOOST_Q_DECIMALS) + 1431)
/** Fixed point format of coefficients. [bits] */
#define BOOST_Q_POLE_DECIMALS  	(5)
/** Q0.Q_POLE_DECIMALS format coefficient. */
#define BOOST_COEFF_A1    		((1 << BOOST_Q_POLE_DECIMALS) + 16)
/** Q0.Q_POLE_DECIMALS format coefficient. */
#define BOOST_COEFF_A0    		(-((0 << BOOST_Q_POLE_DECIMALS) + 16))
/** @} */
/**
 * \name Burst management
 * Frequency limited burst configuration.
 * @{
 */
/** Minimum allowed output of the compensator. [CCU_clock] */
#define BOOST_STNDRD_MIN_DTY    	(BOOST_PWM_DTY_MIN + 14)
#if !(BOOST_STNDRD_MIN_DTY >= BOOST_PWM_DTY_MIN)
	#error "BOOST_STNDRD_MIN_DTY should equal or bigger than BOOST_PWM_DTY_MIN(see comparisons in code)"
#endif
/** Minimum phase applied during burst mode. [CCU_clock] */
#define BOOST_BURST_DTY     		(BOOST_PWM_DTY_MIN + 19)
/** If compensator output falls below this limit the converter stops switching. [CCU_clock] */
#define BOOST_BURST_STOP_THRSHLD	(45)
/** Burst mode activation limit. [CCU_clock] */
#define BOOST_BURST_LOW_THRSHLD  	(55)
/** Burst mode deactivation limit. [CCU_clock] */
#define BOOST_BURST_HIGH_THRSHLD	(65) 
/** @} */

/*---------------------------------------------------------------------------*/
extern void boost_ovp_irq();
/*---------------------------------------------------------------------------*/
/**
 * \brief   Output under voltage protection interrupt.
 * \return  None
 *
 * This will protect against short circuit at the output as well as any abnormal
 * situation leading to a sudden decrease in the output voltage.
 * This is a latching protection.
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_uvp_irq()
{
	uvp_flag = 0x11;
	converter_ctr.status |= (TRAPPING | UV_FAIL);
}
/*---------------------------------------------------------------------------*/
/**
 * \brief   Initialization of startup in boost mode.
 * \return  None
 *
 *  Compensator reference is switched to the startup reference.
 *  Compensator status is reset.
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_start_up_init()
{
	/* Ensures synchronous rectifiers are OFF. */
    pwm_sync_stop();
    /* 
     * In case the output capacitor is still charged the initial reference 
     * would be set at that starting value. 
     */
    uint16_t actual_vin = ADC_VIN_RESULT;
    if(actual_vin > START_UP_INIT)
    {
    	/*
		 * Forces at least one step of start up sequence, there are some 
		 * variables to be updated in this last step of the sequence. 
		 * See start_up_iterate().
		 */
        if(actual_vin >= BOOST_VOLTAGE_REF)
        {
            voltage_boost_ctr.reference = BOOST_VOLTAGE_REF - START_UP_INIT;
        }
        else
        {
        	voltage_boost_ctr.reference = actual_vin - START_UP_INIT;
        }
    }
    else
    {
        voltage_boost_ctr.reference = START_UP_INIT;
    }
    /* Deactivation of burst during soft start */
    voltage_boost_ctr.burst_stop_thrshld = 0;
    voltage_boost_ctr.burst_high_thrshld = 0;
    voltage_boost_ctr.burst_low_thrshld = 0;
    /* Clearing of the compensator status. */
    voltage_boost_ctr.error[0] = 0;
    voltage_boost_ctr.error[1] = 0;
    
    static int32_t duty_init;
    /* Compensation of low initial Vo. */
    if(converter_ctr.adc_vout_measure > BOOST_VO_DTY_BASE)
    {
    	duty_init = BOOST_SOFT_STRT_DUTY_INIT - (((converter_ctr.adc_vout_measure - BOOST_VO_DTY_BASE) * 3) >> 7);
    }
    else
    {
    	duty_init = BOOST_SOFT_STRT_DUTY_INIT;
    }
	/* Compensation of higher initial Vin. */
	if(converter_ctr.adc_vin_measure > BOOST_VIN_MIN_HIGH)
	{
		duty_init = duty_init + (((converter_ctr.adc_vin_measure - BOOST_VIN_MIN_HIGH) * 3) >> 7);
		// if((ADC_VIN_RESULT < (6500)))
		// 	duty_init = duty_init + (((converter_ctr.adc_vin_measure - (BOOST_VIN_MIN_HIGH + 265)) * 3) >> 7);
	}
	/* Range limitation. */
    if(duty_init < 0)
    {
    	duty_init = 0;
    }
    else if(duty_init > BOOST_SOFT_STRT_DUTY_INIT_MAX)
    {
    	duty_init = BOOST_SOFT_STRT_DUTY_INIT_MAX;
    }
// if((ADC_VIN_RESULT < (8757-500*2))&&(duty_init >51))
// 	duty_init = 51;
// if((ADC_VIN_RESULT < (8757-500*2))&&(duty_init >10))
// 	duty_init = 10;
// if((ADC_VIN_RESULT < (5500))&&(duty_init >5))
// 	duty_init = 5;	
duty_init_test = duty_init;
    voltage_boost_ctr.duty[0] = ((BOOST_PWM_DTY_MIN + duty_init) << BOOST_Q_DECIMALS);
    voltage_boost_ctr.duty[1] = ((BOOST_PWM_DTY_MIN + duty_init) << BOOST_Q_DECIMALS);
    /* Reset of the soft start protection timer. */
    voltage_boost_ctr.start_up_cnt = BOOST_START_UP_STEP_TIME;
    /* Minimum phase allowed during soft start */
	voltage_boost_ctr.min_duty = BOOST_STNDRD_MIN_DTY;
	voltage_boost_ctr.burst_duty = BOOST_BURST_DTY;   
	voltage_boost_ctr.sync_strp_ovrlp = BOOST_SYNC_STRP_OVRLP;
	/* 
	 * Initial status would be OFF.
	 * Brown in condition will start the converter in the on-off section. 
	 */
	converter_ctr.status = (BOOST_MODE | START_EN | DCM | SOFT);
}
/*---------------------------------------------------------------------------*/
/**
 * \brief   Input voltage ON OFF control and remote enable feature.
 * \return  None
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_brown_in_off_ctr()
{
    static uint8_t vin_track; /* Hysteresis control of Vin. [Boolean] */
    static uint8_t vout_track; /* Hysteresis control of Vo. [Boolean] */
    static uint32_t previous_tick_v_stable; /* Vo stability filter. [control_cyc] */
    static uint8_t vout_stable; /* Control of stabe Vo before starting. [Boolean] */
    static uint32_t vout_measure_prev; /* Previous iteration Vo measurement. [ADC_units] */
    static uint16_t vout_stable_cnt; /* Vo stability counter. [control_cyc] */
    static uint8_t vin_stable; /* Control of stabe Vin before starting. [Boolean] */
	static uint32_t vin_measure_prev; /* Previous iteration Vin measurement. [ADC_units] */
	static uint16_t vin_stable_cnt; /* Vin stability counter. [control_cyc] */
// uint16_t dely_i_test, dely_j_test;    
    /* 
     * Voltage measurement update. 
     * Note: ADC_VIN_RESULT used for compensation is read synchronous to
     * compensator IRQ in the interruption itself.
     */
    converter_ctr.adc_vin_measure = ((converter_ctr.adc_vin_measure * ADC_VIN_AVG_FACTOR) + ADC_VIN_RESULT) >> ADC_VIN_AVG_RATIO;
    converter_ctr.adc_vout_measure = ((converter_ctr.adc_vout_measure * ADC_VO_AVG_FACTOR) + ADC_VO_RESULT) >> ADC_VO_AVG_RATIO;
    ADC_VO_RESULT_test = ADC_VO_RESULT;
    // ADC_VIN_RESULT_test = ADC_VIN_RESULT;
    /*---------------------------------------------------------------------------*/
	/* 
	 * Vin brown in-out could be deactivated during debugging. 
	 */
	if(!parameters_RAM.debug_vin)
	{
	    /* Control of Vo stable before brown in for appropiate initial duty setting. */
		int32_t vout_variation = converter_ctr.adc_vout_measure - vout_measure_prev;
		/* Control of Vo stable before brown in for appropiate initial duty setting. */
		int32_t vin_variation = converter_ctr.adc_vin_measure - vin_measure_prev;
		/* Quarter a volt variation from previous Vout measurement. */
		if((vout_variation < -VO_STBLTY_MARGIN) || (vout_variation > VO_STBLTY_MARGIN))
		{
			vout_stable = false;
			vout_stable_cnt = 0;
		}
		/* Quarter a volt variation from previous Vout measurement. */
		if((vin_variation < -VIN_STBLTY_MARGIN) || (vin_variation > VIN_STBLTY_MARGIN))
		{
			vin_stable = false;
			vin_stable_cnt = 0;
		}
		if((converter_ctr.time_tick >> V_STBLTY_CLK_DIV) != previous_tick_v_stable)
		{
			previous_tick_v_stable = (converter_ctr.time_tick >> V_STBLTY_CLK_DIV);
			/* Filter of Vout within stable window. */
			if(vout_stable_cnt > V_STBLTY_CNT)
			{
				vout_stable = true;
			}
			else
			{
				vout_stable_cnt ++;
			}
			vout_measure_prev = converter_ctr.adc_vout_measure;
			/* Filter of Vout within stable window. */
			if(vin_stable_cnt > V_STBLTY_CNT)
			{
				vin_stable = true;
			}
			else
			{
				vin_stable_cnt ++;
			}
			vin_measure_prev = converter_ctr.adc_vin_measure;
		}
		/* Next time Vin goes into valid range, it will be while raising. */
		if(converter_ctr.adc_vin_measure < BOOST_VIN_MIN_LOW)
		{
			vin_track = V_RISING;
		}
		/* Next time Vin goes into valid range, it will be while falling. */
		else if(converter_ctr.adc_vin_measure > BOOST_VIN_MAX_HIGH)
		{
			vin_track = V_FALLING;
		}
		/* Next time Vin goes into valid range, it will be while raising. */
		if(converter_ctr.adc_vout_measure < BOOST_VO_MIN_LOW)
		{
			vout_track = V_RISING;
		}
		/* Next time Vin goes into valid range, it will be while falling. */
		else if(converter_ctr.adc_vout_measure > BOOST_VO_MAX_HIGH)
		{
			vout_track = V_FALLING;
		}
		/* If converter not already OFF check conditions for brown out. */
		if(!(converter_ctr.status & OFF))
		{
			/* Switch OFF converter outside of the allowed voltage window. */
			if((converter_ctr.adc_vout_measure < BOOST_VO_MIN_LOW) || (converter_ctr.adc_vout_measure > BOOST_VO_MAX_HIGH))
			// if((converter_ctr.adc_vout_measure > BOOST_VO_MAX_HIGH))
			{
				/* Request the converter to stop. */
				// adc_vout_measure_test = converter_ctr.adc_vout_measure;
				// ADC_VO_RESULT_STOP_test = ADC_VO_RESULT;
				// ADC_VIN_RESULT_STOP_test = ADC_VIN_RESULT;
				// STOP_flag = 0x44;
				converter_ctr.status |= STOP;
			}
		}
		/* If converter is OFF check conditions for brown in. */
		else
		{
			/* Switch ON inside of the allowed input voltage window */
			if(vin_track == V_FALLING)
			{		
				if((vin_stable == true) && (converter_ctr.adc_vin_measure > BOOST_VIN_MIN_LOW) && (converter_ctr.adc_vin_measure < BOOST_VIN_MAX_LOW))
				{
					/* Extra check of Vout in boost mode. */
					if(vout_track == V_FALLING)
					{
						if((vout_stable == true) && (converter_ctr.adc_vout_measure > BOOST_VO_MIN_LOW) && (converter_ctr.adc_vout_measure < BOOST_VO_MAX_LOW))
						{
							/* Request the converter to start. */
							converter_ctr.status |= START;
						}
					}
					else
					{
						if((vout_stable == true) && (converter_ctr.adc_vout_measure > BOOST_VO_MIN_HIGH) && (converter_ctr.adc_vout_measure < BOOST_VO_MAX_HIGH))
						{
							/* Request the converter to start. */
							converter_ctr.status |= START;
						}
					}
				}
			}
			/* Switch ON inside of the allowed input voltage window */
			if(vin_track == V_RISING)
			{	
				if((converter_ctr.status & OFF) && (converter_ctr.adc_vin_measure < BOOST_VIN_MIN_LOW))
				{
					adc_io_set_zero();
				}

				if((vin_stable == true) && (converter_ctr.adc_vin_measure > BOOST_VIN_MIN_HIGH) && (converter_ctr.adc_vin_measure < BOOST_VIN_MAX_HIGH))
				{
					/* Extra check of Vout in boost mode. */
					if(vout_track == V_FALLING)
					{
						if((vout_stable == true) && (converter_ctr.adc_vout_measure > BOOST_VO_MIN_LOW) && (converter_ctr.adc_vout_measure < BOOST_VO_MAX_LOW))
						{
							/* Request the converter to start. */
							converter_ctr.status |= START;
						}
					}
					else
					{
						if((vout_stable == true) && (converter_ctr.adc_vout_measure > BOOST_VO_MIN_HIGH) && (converter_ctr.adc_vout_measure < BOOST_VO_MAX_HIGH))
						{
							/* Request the converter to start. */
							converter_ctr.status |= START;
						}
					}
				}
			}
		}
	}
	else
	{
		/* No brown-in control. */
		if(converter_ctr.status & OFF)
		{
			/* Request the converter to start. */
			converter_ctr.status |= START;
		}
	}
	/*---------------------------------------------------------------------------*/
    /*
     * Remote ON-OFF feature
     */
    if((converter_ctr.status & STOP) || !(converter_ctr.status & START_EN))
    {
		set_off_flag = 0xEE;
		// if(converter_ctr.status & STOP)
		// 	on_off_STOP = 0x55;

		// if(!(converter_ctr.status & START_EN))
		// 	on_off_no_START_EN = 0x66;
    	converter_set_off();
    }
    /* Input voltage monitoring activated, start will come when voltage is within valid range. */
    else if((converter_ctr.status & START))
    {
        /* If no fault happened apart from a requested OFF. */
        if(!(converter_ctr.status & ((STATUS_FAULT | TRAPPING) & ~OFF)) && (converter_ctr.status & OFF))
        {
			// dely_j_test = 10000;
			// while(--dely_j_test);
			// {
			// 	dely_i_test = 50000;
			// 	while(--dely_i_test);
			// }
			boost_start_up_init_count++;
        	boost_start_up_init();
        }
        converter_ctr.status &= ~START;
    }
}
/*---------------------------------------------------------------------------*/
/**
 * \brief   Start up sequence iteration.
 * \return  None
 *
 * Ramps up output voltage reference. Converter increases output voltage in 
 * close loop. Ends of sequence when output voltage reaches nominal value.
 * Time out protection in case of overload.
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_start_up_iterate()
{
	static uint32_t start_up_tick; /* Start up clock divider. [control_cyc] */
	static uint16_t time_out_cnt; /* Filters time out condition. [control_cyc] */
	
    /* Start-up time out protection timer. */
	if(converter_ctr.time_tick != start_up_tick)
	{
		if(voltage_boost_ctr.start_up_cnt > 0)
		{
			voltage_boost_ctr.start_up_cnt --;
		}
		start_up_tick = converter_ctr.time_tick;
	}
    /* Soft star-up routine. */
	if(converter_ctr.status & SOFT)
	{
		/* Time out protection during soft start procedure */
		if((voltage_boost_ctr.start_up_cnt == 0) && !(converter_ctr.status & OFF))
		{
			/* Time out protection can be deactivated during debugging activities */
			if(parameters_RAM.debug_soft)
			{
				/* End of start up sequence in debugging mode. */
				voltage_boost_ctr.reference = BOOST_VOLTAGE_REF;
				voltage_boost_ctr.min_duty = BOOST_STNDRD_MIN_DTY;
				voltage_boost_ctr.burst_duty = BOOST_BURST_DTY;
				voltage_boost_ctr.burst_stop_thrshld = BOOST_BURST_STOP_THRSHLD;
				voltage_boost_ctr.burst_high_thrshld = BOOST_BURST_HIGH_THRSHLD;
				voltage_boost_ctr.burst_low_thrshld = BOOST_BURST_LOW_THRSHLD;
				converter_ctr.status &= ~SOFT;
			}
			/* Latching protection due to possible overload. */
			else if(time_out_cnt > 100)
			{
				converter_ctr.status |= START_FAIL;
				converter_ctr.status |= TRAPPING;
			}
			else
			{
				/* Forces an increment in the reference to avoid the plateau. */
				time_out_cnt ++;
				/* Resets time out timer. */
				voltage_boost_ctr.start_up_cnt = BOOST_START_UP_STEP_TIME;
			}
		}
		/*
		 * Next step will happen after reaching or going over the actual step.
		 * Next step could be forced by first time out during start up, in case of heavy load.
		 */
		if(ADC_VIN_RESULT >= voltage_boost_ctr.reference)
		{
			/* Increment reference only in case no fault is active */
			if(!(converter_ctr.status & STATUS_FAULT))
			{
				voltage_boost_ctr.reference += BOOST_START_UP_INC;
			}
			/* End of start up sequence. */
			if(voltage_boost_ctr.reference >= BOOST_VOLTAGE_REF)
			{
				/* Set of normal mode coefficients. */
				voltage_boost_ctr.reference = BOOST_VOLTAGE_REF;
				voltage_boost_ctr.min_duty = BOOST_STNDRD_MIN_DTY;
				voltage_boost_ctr.burst_duty = BOOST_BURST_DTY;
				voltage_boost_ctr.burst_stop_thrshld = BOOST_BURST_STOP_THRSHLD;
				voltage_boost_ctr.burst_high_thrshld = BOOST_BURST_HIGH_THRSHLD;
				voltage_boost_ctr.burst_low_thrshld = BOOST_BURST_LOW_THRSHLD;
				/* Output voltage protections ON */
				XMC_VADC_GROUP_SetBoundaries(VADC_G1, BOOST_VOLTAGE_UVP, BOOST_VOLTAGE_OVP);
				converter_ctr.status &= ~SOFT;
				time_out_cnt = 0;
			}
			/* Resets time out timer. */
			voltage_boost_ctr.start_up_cnt = BOOST_START_UP_STEP_TIME;
		}
		/*
		 * If soft start is deactivated during debugging activities, 
		 * still output voltage range protection will work after reaching the 
		 * nominal output voltage.
		 */
		if(parameters_RAM.debug_soft)
		{
			/*
			 * Activates the output voltage protections after the reference voltage
			 * is reached.
			 */
			if(ADC_VIN_RESULT >= BOOST_VOLTAGE_REF)
			{
				XMC_VADC_GROUP_SetBoundaries(VADC_G1, BOOST_VOLTAGE_UVP, BOOST_VOLTAGE_OVP);
				converter_ctr.status &= ~SOFT;
			}
		}
	}
	
	/*
	 * Degugging Rg
	 */
//	static uint32_t rg_debug_timeout;
//	if(!(converter_ctr.status & SOFT))
//	{
//		if(rg_debug_timeout > 15000)
//		{
//			converter_ctr.status |= START_FAIL;
//			converter_ctr.status |= TRAPPING;
//		}
//		else
//		{
//			rg_debug_timeout ++;
//		}
//	}
	/*
	 * Debugging Rg
	 */
}
/*---------------------------------------------------------------------------*/
/** Output current level for trapping allowed to avoid Vds overshoot. [ADC_units] */
#define TRAPPING_IO_LVL				(5)
/** Maximum delay of trapping sequence if current does not decrase. [control_cyc] */
#define TRAPPING_MAX_DLY			(30)
#if TRAPPING_MAX_DLY > (BOOST_PWM_DTY_ABS_MAX / 2)
	#error "TRAPPING_MAX_DLY should be lower than half of maximum duty for the soft start trapping sequence to work robust." 
#endif
/** Common mode noise induces measurement error on isolated amplifier. */
#define BOOST_VIN_NOISE_OVERSHOOT	(200)
/*---------------------------------------------------------------------------*/
/**
 * \brief   Iteration of the voltage control loop
 * \return  None
 * \todo    Separate active burst control from control loop iteration to execute control at higher frequency.
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_voltage_ctr_iterate()
{
	static uint16_t burst_in_cnt; /* Burst enter filter counter. [control_cyc] */
	static uint16_t burst_out_cnt; /* Burst exit filter counter. [control_cyc] */
	static uint16_t burst_skip_cnt; /* Burst skipping length sequence counter. [control_cyc] */
	static uint16_t burst_pulse_cnt; /* Burst pulse length sequence counter. [control_cyc] */
	static uint16_t trapping_cnt; /* Trapping maximum delay counter. [control_cyc] */
	int32_t duty; /* New output of compensator. [Q0.Q_DECIMALS] */
	int32_t voltage_error; /* Error of voltage compensator. [Q0.Q_DECIMALS] */
	int16_t duty_out; /* Compensator output in integer value. [Q0] */

    /* Q0 value */
    voltage_error = ((int32_t) voltage_boost_ctr.reference) - ADC_VIN_RESULT;	
    /* Compensator calculations. Q0.Q_DECIMALS result */
// if((converter_ctr.status & SOFT)&&((ADC_VIN_RESULT < (8757-500*2))))
// {
//     duty = (((0 << BOOST_Q_DECIMALS) + 2867 -1000) * voltage_error) + ((-((0 << BOOST_Q_DECIMALS) + 4295-1000)) * voltage_boost_ctr.error[0]) + (BOOST_COEFF_B0 * voltage_boost_ctr.error[1]) + \
//     		(((BOOST_COEFF_A1 * voltage_boost_ctr.duty[0]) + (BOOST_COEFF_A0 * voltage_boost_ctr.duty[1])) >> BOOST_Q_POLE_DECIMALS);
// }
// else
{
    duty = (BOOST_COEFF_B2 * voltage_error) + (BOOST_COEFF_B1 * voltage_boost_ctr.error[0]) + (BOOST_COEFF_B0 * voltage_boost_ctr.error[1]) + \
    		(((BOOST_COEFF_A1 * voltage_boost_ctr.duty[0]) + (BOOST_COEFF_A0 * voltage_boost_ctr.duty[1])) >> BOOST_Q_POLE_DECIMALS);
}
    /* Lower saturation. */
	if(duty < (BOOST_PWM_DTY_MIN << BOOST_Q_DECIMALS))
	{
		duty = (BOOST_PWM_DTY_MIN << BOOST_Q_DECIMALS);
	}
	/* Upper saturation. */
	else if(duty > (BOOST_PWM_DTY_MAX << BOOST_Q_DECIMALS))
	{
		duty = (BOOST_PWM_DTY_MAX << BOOST_Q_DECIMALS);
	}
		
	// if((ADC_VIN_RESULT < (8757-500*2))&&(duty > 110))
	// 		duty = 110;

		// reference_test = voltage_boost_ctr.reference;
		// ADC_VIN_RESULT_test = ADC_VIN_RESULT;
		// if(voltage_error < 0)
		// {
		// 	zhengfu_flag |= 0x10;
		// 	voltage_error_test = - voltage_error;
		// }
		// else
		// {
		// 	zhengfu_flag &= 0x0F;
		// 	voltage_error_test = voltage_error;			
		// }
		// duty = (BOOST_PWM_DTY_MAX << BOOST_Q_DECIMALS);
	/* 
	 * Protections against wrong readings or sudden load changes. 
	 * Note: Common mode noise in isolated amplifier for Vin in hard switching.
	 */
	if((voltage_error > (BOOST_VIN_NOISE_OVERSHOOT)) || voltage_error < (-BOOST_VIN_NOISE_OVERSHOOT)) // Approx. 10V error
	{
		// if(voltage_error < 0)
		// {
		// 	zhengfu_flag |= 0x10;
		// 	voltage_error_test = - voltage_error;
		// }
		// else
		// {
		// 	zhengfu_flag &= 0x0F;
		// 	voltage_error_test = voltage_error;			
		// }
		// BOOST_VIN_NOISE_OVERSHOOT_test = BOOST_VIN_NOISE_OVERSHOOT;
		if(!(converter_ctr.status & OFF))
		{	
			reference_test = voltage_boost_ctr.reference;
			ADC_VIN_RESULT_test = ADC_VIN_RESULT;
			VIN_FAIL_TRAPPING_flag = 0xDD;
			if(ADC_VIN_RESULT < 7000)
				{
					converter_ctr.status |= TRAPPING;
					converter_ctr.status &= ~VIN_FAIL;
				}
			else
			converter_ctr.status |= (TRAPPING | VIN_FAIL);
		}
	}
    /* 
     * Trapping of PWM requires a decreasing duty sequence until current through
     * output choke changes polarity to avoid drain voltage overshoot in secondary
     * rectifiers.
     */
    if(converter_ctr.status & TRAPPING)
    {
		// TRAPPING_flag = 0xDD;
    	/* Decreases gradually duty from previous iteration. */
		// if(duty_out > 0)
		if(voltage_boost_ctr.duty_out > 0)
    	duty_out = voltage_boost_ctr.duty_out - 1;
    	/* Lower saturation during SOFT start. */
    	if(converter_ctr.status & SOFT)
    	{
    		if(duty_out < BOOST_OFF_DTY)
			{
    			if((current_ctr.adc_io_measure < TRAPPING_IO_LVL) || (trapping_cnt > TRAPPING_MAX_DLY))
    			{
    				/* End of decreasing duty sequence. */
    				TRAP_SET();
    				converter_ctr.status &= ~TRAPPING;
    				trapping_cnt = 0;
    			}
    			else
    			{
    				trapping_cnt ++;
    			}
			} 
    	}
    	/* Lower saturation during in normal conditions. */
    	else
    	{
    		if((duty_out < BOOST_OFF_DTY) || (current_ctr.adc_io_measure < TRAPPING_IO_LVL))
			{
				/* End of decreasing duty sequence. */
				TRAP_SET();
				converter_ctr.status &= ~TRAPPING;
				trapping_cnt = 0;
			}    		
    	}
		voltage_boost_ctr.duty_out = duty_out;
		/* Adjustment of secondary rectifiers duty cycle. */
		boost_pwm_sync_shift();
    }
    /* Normal working condition. */
    else
    {
        /* PWM update value comes from the integer part of the compensator output. */
        duty_out = (duty >> BOOST_Q_DECIMALS);

        // if(duty < 0)
        // {
        // 	duty_test = -duty;
        // 	duty_flag = 0x11;
        // }
        // else
        // {
        // 	duty_test = duty;
        // 	duty_flag = 0x00;
        // }
		/* Compensator output under threshold will stop PWM switching. */
		if((duty_out < voltage_boost_ctr.burst_low_thrshld) || ((converter_ctr.status & BURST) && (duty_out < voltage_boost_ctr.burst_high_thrshld)))
		{
			// if(duty_out < 0)
			// {
			// zhengfu_flag |= 0x01;//负
			// duty_out_test = -duty_out;
			// }
			// else
			// {
			// 	zhengfu_flag &= 0xFE;//正
			// 	duty_out_test = duty_out;
			// }
			/* Burst mode condition entrance is filtered. */
			if(burst_in_cnt == 0)
			{ 
				/* Coming from normal mode some settings are required. */
				if(!(converter_ctr.status & BURST))
				{
					/* Burst mode starts with a trapping cycle. */
					// burst_in_cnt_TRAPPING_flag = 0x33;
					converter_ctr.status |= TRAPPING;
					/* Setting counting values at start of burst mode. */
					burst_skip_cnt = BURST_TRAP_CYC;
					burst_pulse_cnt = BURST_PULSE_CYC;
					converter_ctr.status |= BURST;
				}
			}
			else
			{
				burst_in_cnt--;
			}
			/* 
			 * Using different minimum phase values during burst (forces extra
			 * primary current in favor of lower switching loss). 
			 */
			if (converter_ctr.status & BURST)
			{
				/* Limit minimum level of shift. */
				if (duty_out < voltage_boost_ctr.burst_duty)
				{
					voltage_boost_ctr.duty_out = voltage_boost_ctr.burst_duty;
				}
				else
				{
					voltage_boost_ctr.duty_out = duty_out;
				}
			}
			else
			{
				/* Limit control signal may help avoiding hard switching. */
				if (duty_out < voltage_boost_ctr.min_duty)
				{
					voltage_boost_ctr.duty_out = voltage_boost_ctr.min_duty;
				}
				else
				{
					voltage_boost_ctr.duty_out = duty_out;
				}
			}  
			/* Control of number of skipped cycles during burst. */
			if(TRAP_ON)
			{
				/* Under a minimum PWM update value there is no switching (no load). */
				if((burst_skip_cnt == 0) && (duty_out > voltage_boost_ctr.burst_stop_thrshld))
				{
					/* No faults active to continue switching. */
					if(!(converter_ctr.status & STATUS_FAULT))
					{
						TRAP_RST();     
					}
					burst_skip_cnt = BURST_TRAP_CYC;
				}
				else if(burst_skip_cnt > 0)
				{
					burst_skip_cnt --;
				}
			}
			else
			{
				/* Under a minimum PWM update value there is no switching (no load). */
				if(burst_pulse_cnt == 0)
				{
					/* No faults active to continue switching. */
					if(!(converter_ctr.status & STATUS_FAULT))
					{
						// burst_pulse_cnt_TRAPPING_flag = 0x22;
						converter_ctr.status |= TRAPPING;    
					}
					burst_pulse_cnt = BURST_PULSE_CYC;
				}
				else if(burst_pulse_cnt > 0)
				{
					burst_pulse_cnt --;
				}
			}

			/* Adjustment of secondary rectifiers duty cycle. */
			boost_pwm_sync_shift();
			burst_out_cnt = BURST_OUT_CYC;
		}
		else
		{
			/* Burst exit condition is filtered. */
			if(burst_out_cnt == 0)
			{
				/* Coming from burst mode some preparatives are required. */
				if(converter_ctr.status & BURST)
				{
					converter_ctr.status &= ~BURST;
				}
				/* No faults active to continue switching. */
				if((!(converter_ctr.status & STATUS_FAULT)) && TRAP_ON)
				{
					TRAP_RST();
				}
			}
			else
			{
				burst_out_cnt --;
			}
			voltage_boost_ctr.duty_out = duty_out;
			/* Adjustment of secondary rectifiers duty cycle. */
			boost_pwm_sync_shift();
			burst_in_cnt = BURST_IN_CYC;
		}
	    /* Switching ON secondary rectifiers. */
		if((!(converter_ctr.status & STATUS_FAULT)) && (!TRAP_ON))
		{
			boost_pwm_sync_start();
		}	
    }
 
    /* Update PWM signals with new values */
    boost_pwm_shift();
    /* Update of error buffer. */
    voltage_boost_ctr.error[1] = voltage_boost_ctr.error[0];
    voltage_boost_ctr.error[0] = voltage_error;
    /* Storage of current output of the compensator for next iteration. */
    voltage_boost_ctr.duty[1] = voltage_boost_ctr.duty[0];
    voltage_boost_ctr.duty[0] = duty;
}
/*---------------------------------------------------------------------------*/
/**
 * \brief       Constant power limitation curve control.
 * \return      None
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_power_limit_ctr()
{
	static uint32_t dlyd_hiccup_tick; /* Filter of Vout decrement on overpower event. */
    static uint32_t dlyd_hiccup_in_cnt; /* Filter of overpower event. */
    	
	/* Delayed current limitation of converter. */
	if(!(converter_ctr.status & STATUS_FAULT))
	{
		/* Synchronous to control IRQ timer. */
		if((converter_ctr.time_tick >> DLYD_HICCUP_PRSCLR) != dlyd_hiccup_tick)
		{	
			/* Allow current over threshold for certain time. */
			if(current_ctr.adc_io_measure > BOOST_HICCUP_DLYD_LIM)
			{
				/* 
				 * If the output current has been higher than the limit
				 * for several iterations triggers the protection. 
				 */
				if(dlyd_hiccup_in_cnt == 0)
				{
					current_ctr.ovp_state = true;
				}
				else
				{
					dlyd_hiccup_in_cnt --;
				}
			}
			else
			{
				/* Filter of hiccup condition entrance. */
				if(dlyd_hiccup_in_cnt < DLYD_HICCUP_IN_CNT_LIM)
				{
					dlyd_hiccup_in_cnt ++;
				}
			}
			/* Synchronous to control IRQ timer. */
			dlyd_hiccup_tick = (converter_ctr.time_tick >> DLYD_HICCUP_PRSCLR);
		}
	}
	/* Converter is in fault mode. */
	else
	{
		/* Synchronous to control IRQ timer. */
		if((converter_ctr.time_tick >> DLYD_HICCUP_PRSCLR) != dlyd_hiccup_tick)
		{
			/* Filter of hiccup condition entrance. */
			if(dlyd_hiccup_in_cnt < DLYD_HICCUP_IN_CNT_LIM)
			{
				dlyd_hiccup_in_cnt ++;
			}
			/* Reset of flag to allow restart. */
			if(dlyd_hiccup_in_cnt == DLYD_HICCUP_IN_CNT_LIM)
			{
				current_ctr.ovp_state = false;
			}	
			/* Synchronous to control IRQ timer. */
			dlyd_hiccup_tick = (converter_ctr.time_tick >> DLYD_HICCUP_PRSCLR);
		}
	}
}
/*---------------------------------------------------------------------------*/
/**
 * \brief   Iteration of the current control loop
 * \return  None
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_current_ctr_iterate()
{
	static uint16_t hiccup_in_cnt; /* Timing for entering hiccup mode. [control_cyc] */
	static uint32_t hiccup_out_cnt; /* Timing for exiting hiccup mode. [control_cyc] */
	static int16_t adc_io_measure; /* Output current reading. [ADC_units] */
	
	/* 
	 * Average filtering of the input current measurement.
	 */
    current_ctr.adc_il_measure = ((current_ctr.adc_il_measure * ADC_IL_AVG_FACTOR) + ADC_IL_RESULT) >> ADC_IL_AVG_RATIO;
    /* Bidirectional current reading. Extraction of absolute value. */
    adc_io_measure = current_ctr.adc_io_zero - ADC_IO_RESULT;
	if(adc_io_measure < 0)
	{
		adc_io_measure = 0;
	}
	if(converter_ctr.status & SOFT)
		current_ctr.adc_io_measure = ((current_ctr.adc_io_measure * ADC_IO_AVG_FACTOR) + (adc_io_measure)) >> ADC_IO_AVG_RATIO;
	else
    current_ctr.adc_io_measure = ((current_ctr.adc_io_measure * ADC_IO_AVG_FACTOR) + (adc_io_measure << 1)) >> ADC_IO_AVG_RATIO;
    /*
     * Over power protection
     */
    if((current_ctr.adc_io_measure > current_ctr.hiccup_io_limit) || (current_ctr.ovp_state))
    {
        /* If the current has been higher than the hiccup limit for several iterations trigger the protection. */
        if(hiccup_in_cnt == 0)
        {
            /* Switches off the short circuit protection in prevention of a false triggering during hiccup sequence */
            BOOST_OVP_OFF();
			// ovp_off_flag = 0XCC;
            converter_ctr.status |= (TRAPPING | HICCUP_FAIL);
            /* Resets the filter for exiting the hiccup condition */
            hiccup_out_cnt = (uint32_t)HICCUP_OUT_CNT_LIM;
        }
        else
        {
            hiccup_in_cnt --;
        }
    }
    else
    {
        /* Hiccup condition was reached previously */
        if(converter_ctr.status & HICCUP_FAIL)
        {
            /* If the current have been lower than the hiccup limit for certain number of iterations, exits trap. */
            if(hiccup_out_cnt == 0)
            {
            	/* No other fault than HICCUP happened. */
            	if(!(converter_ctr.status & (STATUS_FAULT & ~HICCUP_FAIL)))
            	{
					boost_start_up_init_HICCUP_count++;
            		boost_start_up_init();
                	converter_ctr.status &= ~HICCUP_FAIL;
            	}
            	/* Some other fault active, just remove the HICCUP_FAIL. */
            	else
            	{
            		converter_ctr.status &= ~HICCUP_FAIL;
            	}
            }
            else
            {
                hiccup_out_cnt --;
            }
        }
        /* Filter of Hiccup condition entrance. */
        if(hiccup_in_cnt < BOOST_HICCUP_IN_CNT_LIM)
        {
        	hiccup_in_cnt ++;
        }
    }
}
/*---------------------------------------------------------------------------*/
/**
 * \brief   Vin over and undervoltage protection in boost mode.
 * \return  None
 * 
 * NOTE: Try to implement this by hardware if the scheme works.
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_ovp_uvp_ctr_iterate()
{
	uint32_t adc_vin; /* Reading of Vin without filtering. [ADC_units] */
	
	adc_vin = ADC_VIN_RESULT;
	if(adc_vin > BOOST_VIN_MAX)
	{
		ovp_flag = 0xAA;		
		boost_ovp_irq();
	}
	if((!(converter_ctr.status & (OFF | SOFT))) && (adc_vin < BOOST_VIN_MIN))
	{
        // uvp_flag = 0xBB;		
		boost_uvp_irq();
	}
}
/*---------------------------------------------------------------------------*/
/**
 * \brief       Current ratio of the conduction times adjustment
 * \return      None
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_current_ratio_ctr()
{
	static uint16_t dt_AB_il_adjust; /* Value of dead time updated into the PWM. [CCU_clk] */
	static uint16_t dt_CD_il_adjust; /* Value of dead time updated into the PWM. [CCU_clk] */
	static uint16_t dt_AB_il_inc_cnt; /* Filter of dead time increment. [control_cyc] */
	static uint16_t dt_AB_il_decr_cnt; /* Filter of dead time decrement. [control_cyc] */
	static uint16_t dt_CD_il_inc_cnt; /* Filter of dead time increment. [control_cyc] */
	static uint16_t dt_CD_il_decr_cnt; /* Filter of dead time decrement. [control_cyc] */
	static uint32_t il_previous_tick; /* Input current measurement clock divider. [control_cyc] */
	static uint32_t strtp_prchrg_previous_tick; /* Input current measurement clock divider. [control_cyc] */
	uint16_t dt_AB_il_adjust_new; /* Calculated value of dead time in this iteration. [CCU_clk] */
	uint16_t dt_CD_il_adjust_new; /* Calculated value of dead time in this iteration. [CCU_clk] */

    /* Output overcurrent has to be scaled with regard to the input voltage. */
	current_ctr.hiccup_io_limit = (converter_ctr.status & SOFT) ? BOOST_HICCUP_SOFT_LIM : BOOST_HICCUP_LIM;
	
	/* Time division of the controller clock for bridge deadtimes adjustment. */
	if((converter_ctr.time_tick >> ADST_DLY_PRSCLR) != il_previous_tick)
	{
		/*
		 * NOTE: Adjustive dead times deactivated in boost mode.
		 * Do not really seem to be required so I won't complicate the GUI at
		 * the moment with the new extra parameters, but keep the section here in case of
		 * needed later on.
		 */
		/* Decrement of dead time with increasing current, drain voltage resonates faster for higher loads. */
		dt_AB_il_adjust_new = ((current_ctr.adc_il_measure * 0) >> parameters_RAM.bridge_il_ratio);
		dt_CD_il_adjust_new = ((current_ctr.adc_il_measure * 0) >> parameters_RAM.bridge_il_ratio);
		/* 
		 * Limits the rate of change of the dt to avoid interactions with the
		 * control loop. 
		 */
		if(dt_AB_il_adjust_new > dt_AB_il_adjust)
		{
			if(dt_AB_il_inc_cnt > 0)
			{
				dt_AB_il_inc_cnt --;
			}
			else
			{
				/* Progressive change of dead time. */
				dt_AB_il_adjust ++;
				/* Reset of the change filter. */
				dt_AB_il_inc_cnt = DT_ADJST_DLY;
			}
			/* Reset of the change filter. */
			dt_AB_il_decr_cnt = DT_ADJST_DLY;
		}
		else if(dt_AB_il_adjust_new < dt_AB_il_adjust)
		{
			if(dt_AB_il_decr_cnt > 0)
			{
				dt_AB_il_decr_cnt --;
			}
			else
			{
				/* Progressive change of dead time. */
				dt_AB_il_adjust --;
				/* Reset of the change filter. */
				dt_AB_il_decr_cnt = DT_ADJST_DLY;
			}
			/* Reset of the change filter. */
			dt_AB_il_inc_cnt = DT_ADJST_DLY;
		}
		else
		{
			/* Reset of the change filter. */
			dt_AB_il_decr_cnt = DT_ADJST_DLY;
			dt_AB_il_inc_cnt = DT_ADJST_DLY;
		}
		
		/* 
		 * Limits the rate of change of the dt to avoid interactions with the
		 * control loop. 
		 */
		if(dt_CD_il_adjust_new > dt_CD_il_adjust)
		{
			if(dt_CD_il_inc_cnt > 0)
			{
				dt_CD_il_inc_cnt --;
			}
			else
			{
				/* Progressive change of dead time. */
				dt_CD_il_adjust ++;
				/* Reset of the change filter. */
				dt_CD_il_inc_cnt = DT_ADJST_DLY;
			}
			/* Reset of the change filter. */
			dt_CD_il_decr_cnt = DT_ADJST_DLY;
		}
		else if(dt_CD_il_adjust_new < dt_CD_il_adjust)
		{
			if(dt_CD_il_decr_cnt > 0)
			{
				dt_CD_il_decr_cnt --;
			}
			else
			{
				/* Progressive change of dead time. */
				dt_CD_il_adjust --;
				/* Reset of the change filter. */
				dt_CD_il_decr_cnt = DT_ADJST_DLY;
			}
			/* Reset of the change filter. */
			dt_CD_il_inc_cnt = DT_ADJST_DLY;
		}
		else
		{
			/* Reset of the change filter. */
			dt_CD_il_decr_cnt = DT_ADJST_DLY;
			dt_CD_il_inc_cnt = DT_ADJST_DLY;
		}
		il_previous_tick = (converter_ctr.time_tick >> ADST_DLY_PRSCLR);
	}
	/* Checks whether dead time is still within a valid range. */
	current_ctr.dt_A_B = ((parameters_RAM.boost_dead_time_A_B - dt_AB_il_adjust) > PWM_A_B_MIN_DT) ? (parameters_RAM.boost_dead_time_A_B - dt_AB_il_adjust) : PWM_A_B_MIN_DT;
	current_ctr.dt_C_D = ((parameters_RAM.boost_dead_time_C_D - PWM_C_D_CHAIN_DLY - dt_CD_il_adjust) > PWM_C_D_MIN_DT) ? (parameters_RAM.boost_dead_time_C_D - PWM_C_D_CHAIN_DLY - dt_CD_il_adjust) : PWM_C_D_MIN_DT;

	/* 
	 * Adjustment of the bridge dead times depending on the output current. 
	 * Note: Shadow register transfer request will be done in the IRQ.
	 * */
    pwm_dt_adjust();
    
    /* Time division of the controller clock for bridge dead times adjustment. */
	if((converter_ctr.time_tick >> ADST_OVRLP_PRSCLR) != strtp_prchrg_previous_tick)
	{
		if(voltage_boost_ctr.sync_strp_ovrlp > 0)
		{
			voltage_boost_ctr.sync_strp_ovrlp --;
		}
		strtp_prchrg_previous_tick = (converter_ctr.time_tick >> ADST_OVRLP_PRSCLR);
	}
}
/*---------------------------------------------------------------------------*/
/**
 * \brief	Initialization of the voltage boost controller
 * \return	None
 *
 * State variables may require to be at a defined state before the voltage control
 * starts running.
 */
#ifndef DOXYGEN_SKIP
__attribute__((section(".ram_code")))
#endif
static inline void boost_mode_init()
{
	/* Turns OFF converter during mode exchange. */
	converter_set_off();
	/* Replace control routines by boost converter ones. */
	converter_ctr.status |= BOOST_MODE;
	/* Set synchronous rectifiers in boost mode. */
	buck_to_boost_pwm_cfg();
}
#endif /* __CONTROLLER_H_ */
/** @} */
